package edu.centenary.centenaryController;
import java.util.*;
/**
* Contains the methods to set and retrieve values for each of the robot's sensors.
*/
public class SensorData {
	 /** Changes to true if the robot's touch sensor is touching anything */
    private boolean touch = false;
	 /** An array that stores the range values for the Sonar Sensor */
    private Hashtable<String, Double> sonar = new Hashtable<String, Double>();
	 /** An array that stores the body parts seen by the viticm sensor */
    private ArrayList<BodyPart> parts = new ArrayList<BodyPart>();
	 /** An array that stores the body parts seen by the viticm sensor */
    private ArrayList<WayPoint> waypoint = new ArrayList<WayPoint>();
	 /** An array that stores the available starting locations on the chosen map */
    private ArrayList<Pose> pose = new ArrayList<Pose>();
	 /** An array that stores the range values of the Range Scanner Sensor */
    private double[] rsArray = new double [181];
	  /** This is the roll(Rotation on the X axis) */
    private double roll= 0;
	  /** This is the pitch(Rotation on the Y axis) */
    private double pitch = 0;
	 /** This is the yaw(Rotation on the Z axis) */
    private double yaw = 0; 
	 
	 /** Robot's X coordinate as determined by the INS sensor */
    private double locationX = 0;
	 /** Robot's Y coordinate as determined by the INS sensor */
    private double locationY = 0;
	 /** Robot's Z coordinate as determined by the INS sensor */
    private double locationZ = 0;
    /** Robot's X coordinate as determined by the Odometry sensor */
    private double odLocationX = 0;
	 /** Robot's Y coordinate as determined by the Odometry sensor */
    private double odLocationY = 0;
	 /** Robot's Z coordinate as determined by the Odometry sensor */
    private double odLocationZ = 0;
	 /** Robot's Rotation on the horizontal axis as determined by the Odometry sensor */
    private double odTheta = 0;
	 
	 
    public SensorData() {

    }
	 /**
	 * Sets touch to true or false
	 * @param touch Brings in the true or false touch value from SensorGather
	 */
    public void setTouch(boolean touch) {
        this.touch = touch;
    }
	 /**
	 * Gets stored touch value
	 * @return The true or false touch value
	 */
    public boolean getTouch() {
        return touch;
    }
	 /**
	 * Sets a range value and it's sensor # in an array
	 * @param key Brings in string F1-F8 or R1-R8 representing the robot's front and rear sensors
	 * @param value Brings in the distance in meters between the robot and an obstacle
	 */
    public void setSonar(String key, Double value) {
        sonar.put(key, value);
    }
	 /**
	 * Gets the distance to an obstacle for a specific sonar sensor
	 *@param key A string F1-F8 or R1-R8 representing the robot's front and rear sensors
	 */
    public double getSonar(String key) {
        return sonar.get(key).doubleValue();
    }
	 /**
	 * Gets the distance between the robot and an obstacle
	 * @param i A a value 0-181 representing each Range Scanner Sensor
	 */
    public double getRangeScanner(int i) {
	 	if(i < 0){
			return rsArray[0];
		}else if(i >= rsArray.length){
			return rsArray[rsArray.length -1];
		}else{
        return rsArray[i];
		}
    }
	 /**
	 * Sets the distance between the robot and an obstacle
	 * @param i A a value 0-181 representing each Range Scanner Sensor
	 * @param range the distance in meters between the robot and an obstacle
	 */
    public void setRangeScanner(int i, double range) {
        rsArray[i] = range;
    }
	 /**
	 * Checks to make sure a specific key value is present in the sonar array
	 * @param key A string F1-F8 or R1-R8 representing the robot's front and rear sensors
	 */
    public boolean getSonarKey(String key) {
        return sonar.containsKey(key);
    }
	 /**
	 * Sets the roll value
	 * @param roll A value in radians that represents the rotation about the X axis
	 */
    public void setRoll(double roll) {
        this.roll = roll;
    }
	 /**
	 * Gets the roll value
	 */
    public double getRoll() {
        return roll;
    }
	 /**
	 * Sets the pitch value
	 * @param pitch A value in radians that represents the rotation about the Y axis
	 */
    public void setPitch(double pitch) {
        this.pitch = pitch;
    }
	 /**
	 * Gets the pitch value
	 */
    public double getPitch() {
        return pitch;
    }
	 /**
	 * Sets the yaw value
	 * @param yaw A value in radians that represents the rotation about the Z axis
	 */
    public void setYaw(double yaw) {
        this.yaw = yaw;
    }
	 /**
	 * Get the yaw
	 */
    public double getYaw() {
        return yaw;
    }
	 /**
	 * Sets the locationX variable
	 * @param locationX The robot's location on an X,Y,Z coordinate plane determined by the INS sensor
	 */
    public void setLocationX(double locationX) {
        this.locationX = locationX;
    }
	 /**
	 * Gets locationX
	 * @return Robot's X coordinate
	 */
    public double getLocationX() {
        return locationX;
    }
	 /**
	 * Sets the locationY variable
	 * @param locationY The robot's location on an X,Y,Z coordinate plane determined by the INS sensor
	 */
    public void setLocationY(double locationY) {
        this.locationY = locationY;
    }
	 /**
	 * Gets locationY
	 * @return Robot's Y coordinate
	 */
    public double getLocationY() {
        return locationY;
    }
	 /**
	 * Sets the locationZ variable
	 * @param locationZ The robot's location on an X,Y,Z coordinate plane determined by the INS sensor
	 */
    public void setLocationZ(double locationZ) {
        this.locationZ = locationZ;
    }
	 /**
	 * Gets locationZ
	 * @return Robot's Z coordinate
	 */
    public double getLocationZ() {
        return locationZ;
    }
	 
	 
	 /**
	 * Sets the odLocationX variable
	 * @param locationX The robot's location on an X,Y,Z coordinate plane determined by the Odometry sensor
	 */
    public void setOdLocationX(double odLocationX) {
        this.odLocationX = odLocationX;
    }
	 
	 /**
	 * Gets odLocationX
	 * @return Robot's X coordinate
	 */
    public double getOdLocationX() {
        return odLocationX;
    }
	 /**
	 * Sets the odLocationY variable
	 * @param locationY The robot's location on an X,Y,Z coordinate plane determined by the Odometry sensor
	 */
    public void setOdLocationY(double odLocationY) {
        this.odLocationY = odLocationY;
    }
	 /**
	 * Gets odLocationY
	 * @return Robot's Y coordinate
	 */
    public double getOdLocationY() {
        return odLocationY;
    }
	 /**
	 * Sets the odLocationZ variable
	 * @param locationZ The robot's location on an X,Y,Z coordinate plane determined by the Odometry sensor
	 */
    //public void setOdLocationZ(double odLocationZ) {
    //    this.odLocationZ = odLocationZ;
    //}
	 /**
	 * Gets odLocationZ
	 * @return Robot's Z coordinate
	 */
    //public double getOdLocationZ() {
     //   return odLocationZ;
    //}
	 /**
	 * Sets the odTheta variable
	 * @param odTheta The robot's x axis rotation determined by the Odometry sensor
	 */
    public void setOdTheta (double odTheta) {
        this.odTheta = odTheta;
    }	 
	 /**
	 * Gets odTheta
	 * @return Robot's rotation on the X axis.
	 */
	 public double getOdTheta(){
	 		return odTheta;
	 }
	 /**
	 * Gets odLocationZ
	 * @return Robot's Z coordinate
	 */
    //public double getOdLocationZ() {
      //  return odLocationZ;
	 //}
	 /**
	 * Sets the robot's initial x position
	 * @param x The robot's x initial position as set by the user
	 */
	 //public void setInitialX(x){
	 	//this.initialX = x;
	 //}
	 /**
	 * Gets the robot's initiion x position
	 * @return robot's initial x position as set by the user
	 */
	 //public void getInitialX{
	 	//return initialX;
	 //}
	 /**
	 * Sets the robot's initial x position
	 * @param x The robot's x initial position as set by the user
	 */
	 //public void setInitialY(y){
	 	//this.initialY = y;
	 //}
	 /**
	 * Gets the robot's initiion y position
	 * @return robot's initial y position as set by the user
	 */
	 //public void getInitialY{
	 	//return initialY;
	 //}
	 /**
	 * Sets the robot's initial z position
	 * @param x The robot's z initial position as set by the user
	 */
	 //public void setInitialZ(z){
	 	//this.initialZ = z;
	 //}
	 /**
	 * Gets the robot's initiion z position
	 * @return robot's initial z position as set by the user
	 */
	 //public void getInitialZ{
	 	//return initialZ;
	 //}
	 /**
	 * Sets the robot's initial roll
	 * @param x The robot's roll value as set by the user
	 */
	 //public void setInitialRoll(roll){
	 	//this.initialRoll = roll;
	 //}
	 /**
	 * Gets the robot's initial roll
	 * @return robot's initial roll value as set by the user
	 */
	 //public void getInitialRoll{
	 	//return initialRoll;
	 //}
	 /**
	 * Sets the robot's initial pitch
	 * @param x The robot's pitch value as set by the user
	 */
	 //public void setInitialPitch(pitch){
	 	//this.initialPitch = pitch;
	 //}
	 /**
	 * Gets the robot's inital pitch
	 * @return robot's initial pitch value as set by the user
	 */
	 //public void getInitialPitch{
	 //	return initialPitch;
	 //}
	 /**
	 * Sets the robot's initial yaw
	 * @param x The robot's yaw value as set by the user
	 */
	 //public void setInitialYaw(yaw){
	 	//this.initialYaw = yaw;
	 //}
	 /**
	 * Gets the robot's inital yaw
	 * @return robot's initial yaw value as set by the user
	 */
	 //public void getInitialYaw{
	 //	return initialYaw;
	 //}
	 /**
	 * Adds Pose object to pose array
	 * @param p A pose object which contains the X,Y,Z and roll,pitch,yaw values for map starting locations
	 */
    public void addPose(Pose p) {
        pose.add(p);
    }
	 /**
	 * Gets a specific pose object
	 * @param i An identifying number assigned to each pose as it is generated
	 * @return The X,Y,Z and roll,pitch,yaw values for map starting locations
	 */
    public Pose getPose(int i) {
        return pose.get(i);
    }
	 /**
	 * Gives the size of the pose array
	 * @return The number of poses in the array
	 */
    public int numberOfPoses() {
        return pose.size();
    }  
	 /**
	 * Gives the name and location of all limbs the robot has detected
	 * @return The parts array which contains the name and x,y,z location of each limb
	 */
    public ArrayList<BodyPart> getBodyPartList() {
        return parts;
    }
	 /**
	 * Resets the body part list to it's new state
	 */
	 public void resetParts(){
	 	parts = new ArrayList<BodyPart>();
	 }
	 /**
	 * Creates BodyPart objects when the robot's Victim Sensor detects a limb, and adds it the the parts array if it hasn't already been identified
	 * @param bodyPart The name of the limb found (ex. Arm, Leg, Pelvis)
	 * @param bodyPartLocationX The X coordinate of the limb relative to the robot's rotation
	 * @param bodyPartLocationY The Y coordinate of the limb relative to the robot's rotation
	 * @param bodyPartLocationZ The Z coordinate of the limb relative to the robot's rotation
	 */
    public void setVictSense(String bodyPart, double bodyPartLocationX, double bodyPartLocationY, double bodyPartLocationZ ) {
	 		BodyPart bp = new BodyPart(bodyPart, bodyPartLocationX, bodyPartLocationY, bodyPartLocationZ);
         parts.add(bp);
    }
	 
	 /**
	 * Gives the length of the RangeScanner array
	 * @return The length of the rsArray
	 */ 
    public int getRSArrayLength() {
        return rsArray.length;
    }

}